from LagrangianParticles import zalesak
from dolfin import *
from numpy import sin, cos, array


'''Rotate zalesak disk by velocity field (-a(y-b),a(x-b)).'''

def build(points):
    '''Given ordered points buid boundary mesh and the level-set function.''' 
    N = len(points)
    
    mesh = Mesh()
    editor = MeshEditor()
    editor.open(mesh, 1, 2)
    editor.init_vertices(N)
    editor.init_cells(N)

    for i, (x, y) in enumerate(points):
        editor.add_vertex(i, x, y)
    for i in range(N):
        editor.add_cell(i, i, (i+1) % (N))

    rect = RectangleMesh(-50,-50,50,50,100,100)
    V = FunctionSpace(rect, 'CG', 1)
    x0 = interpolate(Expression('x[0]'), V)
    x1 = interpolate(Expression('x[1]'), V)
    a0 = x0.vector().array()
    a1 = x1.vector().array()

    # For all nodes in V find the signed distance to the disc 
    point_outside = Point(-50,-50)  # A point outside the disc
    Np = x0.vector().size()
    dummymesh = Mesh()
    dummyeditor = MeshEditor()
    dummyeditor.open(dummymesh, 1, 2)
    dummyeditor.init_vertices(Np + 1)
    dummyeditor.init_cells(Np)
    dummyeditor.add_vertex(0, point_outside.x(), point_outside.y())

    inside = Function(V)
    inside.vector()[:] = 1
    # Use ray casting to determine if the point is inside (-1) or outside (+1)
    # There's probably a better way to do this than to create a dummy mesh
    for i in range(Np):
        dummyeditor.add_vertex(i + 1, a0[i], a1[i])
        dummyeditor.add_cell(i, 0, i + 1)
        ei = Edge(dummymesh, i) # Edge (ray) from vertex 0 to i + 1
        ix = mesh.intersected_cells(ei) # Odd number means you're inside
        if ix.shape[0] % 2 == 1:
            inside.vector()[i] = -1
      
    phiX_ = Function(V)

    i = 0
    # Compute the signed distance
    for xx, yy in zip(a0, a1):
        p = Point(xx, yy)
        zz = mesh.distance(p)
        phiX_.vector()[i] = zz*inside.vector()[i]
        i += 1

    return mesh,phiX_

def rotate(points,a,b,t):
    '''Rotate points by velocity field (-a(y-b),a(x-b)). Time step t'''
    for point in points:
        point[:] = array([b,b]) - b*array([cos(a*t)-sin(a*t),cos(a*t)+sin(a*t)]) + array([cos(a*t)*point[0]-sin(a*t)*point[1],
                                                                                          sin(a*t)*point[0]+cos(a*t)*point[1]])
points = zalesak(center=(0, 25),radius=15,width=5,slot_length=25,N=50)

t = 0
dt = 0.1
step = 0
out = File('zalesak.pvd')
while t < 8:
    rotate(points,-1,5,dt)
    bdry_mesh, sdf = build(points)
    out << sdf, step
    #interactive()
    t += dt
    step += 1


